#ifndef APRILTAG_LOCATION_H_
#define APRILTAG_LOCATION_H_

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PointStamped.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/Imu.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/buffer.h>

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <apriltag.h>
#include "apriltag_ros/AprilTagDetection.h"
#include "apriltag_ros/AprilTagDetectionArray.h"

#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <cv_bridge/cv_bridge.h>

#include <multi_camera_cooperation/colors.h>
#include "multi_camera_cooperation/math_tools.h"

#include <cstdlib>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cstdio>
#include <fstream>
#include <cmath>
#include <ctime>
#include <queue>
#include <vector>
#include <chrono>
#include <sstream>
#include <yaml-cpp/yaml.h>

class AprilTagLocationNodeROS;

class AprilTagLocationNodeROS
{
public:
    struct AprilTagLocation
    {
        int id;
        double weight;
        Eigen::Vector3d position;
        Eigen::Quaterniond orientation;
        Eigen::Vector3d origen_position;
    };

    struct apriltag_package
    {
        int id;
        Eigen::Matrix4d offset;
    };

//================== basic params ===================//
    int apriltag_num;
    std::string cam;
    std::string apriltag_location_topic;
    std::string config_file_path;
    int minitag_id;
    apriltag_ros::AprilTagDetectionArray apriltag_detection_array;
    std::vector<AprilTagLocation> apriltag_location_list;
    std::vector<apriltag_package> apriltag_package_list;

//====================== ros ========================//
    ros::Subscriber apriltag_detection_sub;
    ros::Publisher apriltag_location_pub;
    ros::Publisher apriltag_estimation_pub;
    apriltag_ros::AprilTagDetectionArray::ConstPtr apriltag_detection_msg;

//====================== proscess params ========================// 
    Eigen::Matrix4d T_base_to_estimation = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_origin = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_cam = Eigen::Matrix4d::Identity();
    Eigen::Vector3d fusion_position = Eigen::Vector3d::Zero();
    Eigen::Quaterniond fusion_orientation = Eigen::Quaterniond::Identity();
    Eigen::Vector3d fusion_origin_position = Eigen::Vector3d::Zero();

    Eigen::Matrix4d correction_matrix = Eigen::Matrix4d::Identity();

///------------- function declarations -------------///
    AprilTagLocationNodeROS(ros::NodeHandle &nh);

    void apriltags_callback(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg);

    void location_solve();

    void LoadConfig(std::string config_file_path, std::string cam_name, int apriltag_num);

    Eigen::Quaterniond QuaternionsClose_Merge(std::vector<AprilTagLocation> &tag);
};

#endif // APRILTAG_LOCATION_H_